// 加到五个舵机前
#include "my_robot_hardware/zc4_hardware_interface.hpp"

namespace zc4_hardware {

hardware_interface::CallbackReturn Zc4HardwareInterface::on_init(
  const hardware_interface::HardwareInfo & info)
{
  if (hardware_interface::SystemInterface::on_init(info) !=
      hardware_interface::CallbackReturn::SUCCESS)
  {
    return hardware_interface::CallbackReturn::ERROR;
  }

  info_ = info;

  left_motor_id_ = 10;
  right_motor_id_ = 20;
  port_ = "/dev/ttyUSB0";

  driver_ = std::make_shared<XM430Driver>(port_);

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn Zc4HardwareInterface::on_configure(
  const rclcpp_lifecycle::State & previous_state)
{
  (void)previous_state;
  if (driver_->init() != 0) {
    return hardware_interface::CallbackReturn::ERROR;
  }
// +++ 添加这一行，初始化批量读写 +++
  if (!driver_->initBulkSync(left_motor_id_, right_motor_id_)) {
      std::cout << "Failed to initialize Bulk/Sync handlers!" << std::endl;
      return hardware_interface::CallbackReturn::ERROR;
  }

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn Zc4HardwareInterface::on_activate(
  const rclcpp_lifecycle::State & previous_state)
{
  (void)previous_state;
  set_state("joint11/position", 0.0);
  set_state("joint2/position", 0.0);
  set_state("joint3/position", 0.0);
  set_state("joint4/position", 0.0);
  set_state("joint5/position", 0.0);
  driver_->activateWithPositionMode(left_motor_id_);
  driver_->activateWithPositionMode(right_motor_id_);
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn Zc4HardwareInterface::on_deactivate(
  const rclcpp_lifecycle::State & previous_state)
{
  (void)previous_state;
  driver_->deactivate(left_motor_id_);
  driver_->deactivate(right_motor_id_);
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type Zc4HardwareInterface::read(
  const rclcpp::Time & time, const rclcpp::Duration & period)
{
  (void)time;
  (void)period; // 添加(void)period; 避免编译器警告

    double left_pos = 0.0;
    double right_pos = 0.0;

    // 使用新的批量读函数
    if (driver_->readPresentPositions(left_motor_id_, left_pos, right_motor_id_, right_pos))
    {
      set_state("joint11/position", left_pos);
      set_state("joint2/position", right_pos);
    }
    // 你可以加一个 else 来处理读取失败
  return hardware_interface::return_type::OK;
}

hardware_interface::return_type Zc4HardwareInterface::write(
  const rclcpp::Time & time, const rclcpp::Duration & period)
{
  (void)time;
  (void)period;
// 使用新的同步写函数
  driver_->writeTargetPositions(
    left_motor_id_, get_command("joint11/position"),
    right_motor_id_, get_command("joint2/position")
  );
  return hardware_interface::return_type::OK;
}

}  // namespace zc4_hardware

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(zc4_hardware::Zc4HardwareInterface, hardware_interface::SystemInterface)